/*==============================================================================
            Copyright (c) 2013 Moritz Hader, Marcel Schrder
            All Rights Reserved.

	@file  HeimdallMath.cpp

	@brief Provides math-utilities the HeimdallARBrowser uses to transform matrices
			and make other linear-algebra calculations.
   
   This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

==============================================================================*/

#include <egl/HeimdallMath.h>
#include <egl/HeimdallGLUtilities.h>

#include <math.h>

//TODO: Math Functions
#define A(row,col)  a[(col<<2)+row]
#define B(row,col)  b[(col<<2)+row]
#define P(row,col)  product[(col<<2)+row]

///////////////////////////////////////////////////////////////////////////////
// Multiply two 4x4 matricies
// This method was copied from the math3d library of GLTools - a is multiplied by b
///////////////////////////////////////////////////////////////////////////////
void H3DMatrixMultiply44(H44DMatrix product, const H44DMatrix a, const H44DMatrix b )
{
	for (int i = 0; i < 4; i++) {
		float ai0=A(i,0),  ai1=A(i,1),  ai2=A(i,2),  ai3=A(i,3);
		P(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
		P(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
		P(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
		P(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
	}
}

#undef A
#undef B
#undef P


void MakeInitialMatrix(H44DMatrix matrix){

	for(int i = 0; i<16; i++){
		matrix[i] = 0.0f;
	}

	//Is there a better way? Dunno, maybe there is.
	matrix[0] = 1.0f;
	matrix[5] = 1.0f;
	matrix[10] = 1.0f;
	matrix[15] = 1.0f;
}

void scaleMatrix(H44DMatrix matrix, H3DVector scale){

	matrix[0] *= scale[0];
	matrix[1] *= scale[0];
	matrix[2] *= scale[0];
	matrix[3] *= scale[0];

	matrix[4] *= scale[1];
	matrix[5] *= scale[1];
	matrix[6] *= scale[1];
	matrix[7] *= scale[1];

	matrix[8] *= scale[2];
	matrix[9] *= scale[2];
	matrix[10] *= scale[2];
	matrix[11] *= scale[2];
}

void rotateMatrix(H44DMatrix matrix, H3DVector rotate){

	H44DMatrix rotation, result;
	H3DVector axis;

	MakeInitialMatrix(rotation);

	if(rotate[0] != 0.0f){
		axis[0] = 1.0f;
		axis[1] = 0.0f;
		axis[2] = 0.0f;

		GenerateRotationMatrix(rotate[0],axis, rotation);
		H3DMatrixMultiply44(result,matrix,rotation);
		memcpy(matrix, result, sizeof(H44DMatrix));
	}

	if(rotate[1] != 0.0f){
		axis[0] = 0.0f;
		axis[1] = 1.0f;
		axis[2] = 0.0f;

		GenerateRotationMatrix(rotate[1],axis, rotation);
		H3DMatrixMultiply44(result,matrix,rotation);
		memcpy(matrix, result, sizeof(H44DMatrix));
	}

	if(rotate[2] != 0.0f){
		axis[0] = 0.0f;
		axis[1] = 0.0f;
		axis[2] = 1.0f;

		GenerateRotationMatrix(rotate[2], axis, rotation);
		H3DMatrixMultiply44(result,matrix,rotation);
		memcpy(matrix, result, sizeof(H44DMatrix));
	}



}

/**
 * Generates the rotation matrix using the given angle and an axis, around which the rotation is
 * performed. The angle must be provided as Degree, as it is automatically transformed to Radiant.
 * Please note that the ray should be of the length 1 (unit-vector)
 */
void GenerateRotationMatrix(float angle, H3DVector axis, H44DMatrix matrix)
{
		if(axis[0] == 0.0f && axis[1] == 0.0f && axis[2] == 0.0f){
			//The axis is 0, so nothing to do here
			return;
		}

		float sine, cosine;

			sine = sin(angle*HD_D2R); //HD_D2R is a constant that represents PI/180 to gain performance
			cosine = cos(angle*HD_D2R);

		float len;

			len = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); //Note, that axis is resembled by the coordinates (x,y,z)

		//Next, we adjust the axis to be of length 1 if necessary
		if(len != 1.0f){
			axis[0]/=len;
			axis[1]/=len;
			axis[2]/=len;
		}


	//Precompile the matrix access for better performance
	#define Mat(r,c)  matrix[r+4*c] //For every row, there are 4 columns, thus rownumber + columnnumber*4

		//As the compiler will create these variables anyway, we can just create them to ease the understanding of this algorithm
		float xSq = axis[0]*axis[0];
		float ySq = axis[1]*axis[1];
		float zSq = axis[2]*axis[2];

		float xMy = axis[0]*axis[1];
		float yMz = axis[1]*axis[2];
		float zMx = axis[2]*axis[0];

		//The Formular also contains a 1-cos(D) term several times. Well precalculate this too to ease the work of the optimizer
		float cos1 = 1.0f-cosine;

		//The last element that occures several times, are the multiplications of the axis with sine
		float xSine = axis[0]*sine;
		float ySine = axis[1]*sine;
		float zSine = axis[2]*sine;

		//Perform the necessary calculations
		Mat(0,0) = cosine+(xSq*cos1); //cos(angle)+x^2 * (1-cos(angle))
		Mat(1,0) = (xMy*cos1)+zSine; //xy*(1-cos(angle))+z*sin(angle)
		Mat(2,0) = (zMx*cos1)-ySine; //xz*(1-cos(angle))-y*sin(angle)
		Mat(3,0) = 0.0f;

		Mat(0,1) = (xMy*cos1)-zSine; //xy*(1-cos(angle))-z*sin(angle)
		Mat(1,1) = cosine+(ySq*cos1); //cos(angle)+y^2 * (1-cos(angle))
		Mat(2,1) = (yMz*cos1)+xSine; //zy*(1-cos(angle))+x*sin(angle)
		Mat(3,1) = 0.0f;

		Mat(0,2) = (zMx*cos1)+ySine; //xz*(1-cos(angle))+y*sin(angle)
		Mat(1,2) = (yMz*cos1)-xSine; //zy*(1-cos(angle))-x*sin(angle)
		Mat(2,2) = cosine+(zSq*cos1);
		Mat(3,2) = 0.0f;

		//As allways, the last column is empty (except for the last element)
		Mat(0,3) = 0.0f;
		Mat(1,3) = 0.0f;
		Mat(2,3) = 0.0f;
		Mat(3,3) = 1.0f;

	#undef Mat

}

void translateMatrix(H44DMatrix matrix, H3DVector translate){

	/*#define Mat(r,c)  matrix[r+4*c]

	float transR1 = Mat(0,0)*translate[0]+Mat(0,1)*translate[1]+Mat(0,2)*translate[2];
	float transR2 = Mat(1,0)*translate[0]+Mat(1,1)*translate[1]+Mat(1,2)*translate[2];;
	float transR3 = Mat(2,0)*translate[0]+Mat(2,1)*translate[1]+Mat(2,2)*translate[2];;
	float transR4 = Mat(3,0)*translate[0]+Mat(3,1)*translate[1]+Mat(3,2)*translate[2];;

	Mat(0,3) += transR1;
	Mat(1,3) += transR2;
	Mat(2,3) += transR3;
	Mat(3,3) += transR3;

	#undef Mat*/

	float x = translate[0]; float y = translate[1]; float z = translate[2];

	 matrix[12] +=
	        (matrix[0] * x + matrix[4] * y + matrix[8]  * z);

	    matrix[13] +=
	        (matrix[1] * x + matrix[5] * y + matrix[9]  * z);

	    matrix[14] +=
	        (matrix[2] * x + matrix[6] * y + matrix[10] * z);

	    matrix[15] +=
	        (matrix[3] * x + matrix[7] * y + matrix[11] * z);
}
